#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include "nav_msgs/Odometry.h"
#include "quadrotor_msgs/PositionCommand.h"
#include "geometry_msgs/PoseStamped.h"
#include "visualization_msgs/Marker.h"
#include "nav_msgs/Path.h"
#include "formation_avoidance/formation_structure.h"
Eigen::Vector3d start_centre(0,0,0);
int send_count=0;

Eigen::Matrix3d rotate_matrix;

double flied_traj_lock_time=0;
std::vector<Eigen::Vector3d> current_p;
std::vector<Eigen::Vector3d> pos;
std::vector<Eigen::Vector3d> send_start_pos;
std::vector<nav_msgs::Path> flied_traj;
double start_yaw=0;

int agent_num;

void startcb(geometry_msgs::PoseStamped msg)
{
	start_centre(0)=msg.pose.position.x;
	start_centre(1)=msg.pose.position.y;
	start_centre(2)=0;

	send_count=0;

	Eigen::Quaterniond quaternion(msg.pose.orientation.w,msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z);
   

	start_yaw=std::atan2(msg.pose.orientation.z,msg.pose.orientation.w)*2;

	rotate_matrix=Eigen::Matrix3d(quaternion);

	for(int id=0;id<agent_num;id++)
	{
		send_start_pos[id]=(rotate_matrix*pos[id]+start_centre);
		ROS_INFO("send_start_pos: %f %f %f",send_start_pos[id](0),send_start_pos[id](1),send_start_pos[id](2));
		flied_traj[id].poses.clear();

	}

	flied_traj_lock_time=ros::Time::now().toSec();

}

void agentcb0(nav_msgs::Odometry msg)
{
	current_p[0]<<msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z;


    flied_traj[0].header.stamp=ros::Time::now();
    flied_traj[0].header.frame_id="world";
    geometry_msgs::PoseStamped point;
    point.pose.position.x = current_p[0](0);
    point.pose.position.y = current_p[0](1);
    point.pose.position.z = current_p[0](2);
    point.header.stamp=ros::Time::now();
    point.header.frame_id="world";
	if(ros::Time::now().toSec()-flied_traj_lock_time>1.0)
		flied_traj[0].poses.push_back(point);


}


void agentcb1(nav_msgs::Odometry msg)
{
	current_p[1]<<msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z;

    flied_traj[1].header.stamp=ros::Time::now();
    flied_traj[1].header.frame_id="world";
    geometry_msgs::PoseStamped point;
    point.pose.position.x = current_p[1](0);
    point.pose.position.y = current_p[1](1);
    point.pose.position.z = current_p[1](2);
    point.header.stamp=ros::Time::now();
    point.header.frame_id="world";
	if(ros::Time::now().toSec()-flied_traj_lock_time>1.0)
		flied_traj[1].poses.push_back(point);


}


void agentcb2(nav_msgs::Odometry msg)
{
	current_p[2]<<msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z;

    flied_traj[2].header.stamp=ros::Time::now();
    flied_traj[2].header.frame_id="world";
    geometry_msgs::PoseStamped point;
    point.pose.position.x = current_p[2](0);
    point.pose.position.y = current_p[2](1);
    point.pose.position.z = current_p[2](2);
    point.header.stamp=ros::Time::now();
    point.header.frame_id="world";

	if(ros::Time::now().toSec()-flied_traj_lock_time>1.0)
	    flied_traj[2].poses.push_back(point);
}

void agentcb3(nav_msgs::Odometry msg)
{
	current_p[3]<<msg.pose.pose.position.x,msg.pose.pose.position.y,msg.pose.pose.position.z;

    flied_traj[3].header.stamp=ros::Time::now();
    flied_traj[3].header.frame_id="world";
    geometry_msgs::PoseStamped point;
    point.pose.position.x = current_p[3](0);
    point.pose.position.y = current_p[3](1);
    point.pose.position.z = current_p[3](2);
    point.header.stamp=ros::Time::now();
    point.header.frame_id="world";

	if(ros::Time::now().toSec()-flied_traj_lock_time>1.0)
		flied_traj[3].poses.push_back(point);
}

geometry_msgs::Point getPointFromEigen(Eigen::Vector3d pos)
{
	geometry_msgs::Point point;
	point.x=pos(0);
	point.y=pos(1);
	point.z=pos(2);
	return point;

}


int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "setStartButton");
    ros::NodeHandle nh("~");
	nh.getParam("/AGENT_NUM",agent_num);

	ros::Rate looprate(30);
	std::vector<ros::Publisher> restart(agent_num);

	for(int id=0;id<agent_num;id++)
	{
		std::string topic="/drone_"+std::to_string(id)+"_planning/pos_cmd";
		restart[id]=nh.advertise<quadrotor_msgs::PositionCommand>(topic, 1, true);

	}

	// restart[0]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_0_planning/pos_cmd", 1, true);
	// restart[1]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_1_planning/pos_cmd", 1, true);
	// restart[2]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_2_planning/pos_cmd", 1, true);
	// restart[3]=nh.advertise<quadrotor_msgs::PositionCommand>("/drone_3_planning/pos_cmd", 1, true);

    ros::Subscriber start_sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, startcb);

	ros::Subscriber agent0_sub=nh.subscribe<nav_msgs::Odometry>("/drone_0_visual_slam/odom", 1, agentcb0);
	ros::Subscriber agent1_sub=nh.subscribe<nav_msgs::Odometry>("/drone_1_visual_slam/odom", 1, agentcb1);
	ros::Subscriber agent2_sub=nh.subscribe<nav_msgs::Odometry>("/drone_2_visual_slam/odom", 1, agentcb2);
	ros::Subscriber agent3_sub=nh.subscribe<nav_msgs::Odometry>("/drone_3_visual_slam/odom", 1, agentcb3);

	ros::Publisher formation_pub=nh.advertise<visualization_msgs::Marker>("/formation", 1, true);

	std::vector<ros::Publisher> flied_traj_visualization_pub(agent_num);


	for(int id=0;id<agent_num;id++)
	{
		std::string topic="/uav"+std::to_string(id)+"/flied_traj";
		flied_traj_visualization_pub[id]=nh.advertise<nav_msgs::Path>(topic, 1, true);
	}

    // ros::Publisher flied_traj_visualization_pub0 = nh.advertise<nav_msgs::Path>("/uav0/flied_traj", 1);
    // ros::Publisher flied_traj_visualization_pub1 = nh.advertise<nav_msgs::Path>("/uav1/flied_traj", 1);
    // ros::Publisher flied_traj_visualization_pub2 = nh.advertise<nav_msgs::Path>("/uav2/flied_traj", 1);
    // ros::Publisher flied_traj_visualization_pub3 = nh.advertise<nav_msgs::Path>("/uav3/flied_traj", 1);


	int formation_type;

    nh.getParam("/FORMATION_TYPE",formation_type);

	double formation_edge_ratio,formation_safety_distance,formation_base_edge_length;

    nh.getParam("/formation_edge_ratio",formation_edge_ratio);
    nh.getParam("/formation_safety_distance",formation_safety_distance);
    nh.getParam("/formation_base_edge_length",formation_base_edge_length);

	formation_avoidance::Formation_Structure formation_info;

	formation_info.formation_structure_init(formation_avoidance::formation(formation_type),formation_edge_ratio,formation_safety_distance,
												formation_base_edge_length);

	ros::Time start_time=ros::Time::now();

	Eigen::Vector3d pos0=Eigen::Vector3d::Zero();
	Eigen::Vector3d pos1;
	
	if(formation_type==formation_avoidance::Diamond||formation_type==formation_avoidance::Vertical_Line)
		pos1<<-formation_base_edge_length,0,0;
	if(formation_type==formation_avoidance::Rectangle)
		pos1<<0,-formation_base_edge_length,0;
	if(formation_type==formation_avoidance::Hexagon)
		pos1<<formation_base_edge_length,0,0;



	pos=formation_info .calculateAllPosition(pos0,pos1);

	agent_num=formation_info.formation_agent_num;

	send_start_pos=pos;
	flied_traj.resize(agent_num);
	current_p.resize(agent_num);

	while (1)
	{
		if(send_count<30)
		{
			for(int id=0;id<agent_num;id++)
			{
				quadrotor_msgs::PositionCommand setpose;
				setpose.header.stamp=ros::Time::now();
				setpose.position.x=send_start_pos[id](0);
				setpose.position.y=send_start_pos[id](1);
				setpose.position.z=send_start_pos[id](2);
				setpose.yaw=start_yaw;
				restart[id].publish(setpose);
			}
		}
		send_count++;
		send_count=std::min(1000,send_count);


		visualization_msgs::Marker line;
		line.header.stamp=ros::Time::now();
		line.id=3084;
		line.type=visualization_msgs::Marker::LINE_STRIP;
		line.header.frame_id = "map";
		line.action = visualization_msgs::Marker::ADD;
		line.ns	 = "lines_and_points";
		line.lifetime = ros::Duration(0);
		line.scale.x=0.09;
		line.color.r=0.5;
		line.color.g=0.5;
		line.color.b=0.5;
		line.color.a=1.0;
		int list[8]={0,1,2,3,0,2,1,3};
		for(int i=0;i<8;i++)
		{
			geometry_msgs::Point vertex=getPointFromEigen(current_p[list[i]]);
			line.points.push_back(vertex);
		}
		formation_pub.publish(line);
		
		for(int id=0;id<agent_num;id++)
		{
			flied_traj_visualization_pub[id].publish(flied_traj[id]);
		}

		looprate.sleep();
		ros::spinOnce();


	}
	
	


}